#pragma once
#include <highgui.h>


namespace CalibrationResultTester
{
	CvPoint Convert(CvMat* objectPoint, CvMat* camera, CvMat* rotation, CvMat* translation)
	{		
		CvMat* S = cvCreateMat(3, 3, CV_32F);
		CvMat* M = cvCreateMat(3, 3, CV_32F);

		CvMat* R = cvCreateMat(3,3, CV_32F);
        cvRodrigues2(rotation, R);

		printf("\r\n----------\r\n");
		for (int i = 0; i < 9;i++)
			printf("%f, ",R->data.fl[i]);
		printf("\r\n----------\r\n");

		float r11 = R->data.fl[0];
		float r12 = R->data.fl[1];
		float r13 = R->data.fl[2];

		float r21 = R->data.fl[3];
		float r22 = R->data.fl[4];
		float r23 = R->data.fl[5];
				
		float r31 = R->data.fl[6];
		float r32 = R->data.fl[7];
		float r33 = R->data.fl[8];

		CvPoint3D32f* o = (CvPoint3D32f*)objectPoint->data.fl;

		float x = r11*o->x + r12*o->y + r13*o->z + translation->data.fl[0];
		float y = r21*o->x + r22*o->y + r23*o->z + translation->data.fl[1];
		float z = r31*o->x + r32*o->y + r33*o->z + translation->data.fl[2];
		
		float fx = camera->data.fl[0];
		float fy = camera->data.fl[4];
		float cx = camera->data.fl[2];
		float cy = camera->data.fl[5];

		z = z ? 1/z : 1;
		x = fx*x*z + cx;
		y = fy*y*z + cy;
		
		printf("Self calculation:");
		printf("[%f, %f]", x,y);
		
		cvReleaseMat(&R);
		cvReleaseMat(&M);
		cvReleaseMat(&S);
		return cvPoint(0,0);
	}

	void Test()
	{
		float cameraData[] = {	 5.7291609867013528e+002, 0., 3.2610417927354649e+002, 0.,
       5.8173582743077156e+002, 2.6320080617076576e+002, 0., 0., 1. };

		CvMat camera = cvMat( 3, 3, CV_32F, &cameraData );

		// Object Points
		int point_count = 1;
		float pointsData[] = {1,1,0};
		CvMat objectPoints = cvMat( 1, point_count, CV_32FC3, pointsData );

		// Rotation vector
		float rotationData[] = {-2.35292006e+000, -1.93340540e-001, 2.16562986e-001};
		CvMat rotation = cvMat( 1, 3, CV_32F, &rotationData);

		// Translation vector
		float translationData[] = {-1.83015060e+000, 1.80811465e+000, 1.44920006e+001};
		CvMat translation = cvMat( 1, 3,CV_32F, &translationData);

		// Discortion coeficients
		double distortionData[] = {   -3.4904083933143709e-001, 1.2404147547446789e-001, -1.3594438773272233e-002, 7.8754140384982539e-003 };
		CvMat distortion = cvMat( 1, 4,CV_64F, &distortionData);

		// Result point on screen
		CvMat* imagePoints = cvCreateMat( 1, 1, CV_32FC2);

		// Convert by OpneCV
		cvProjectPoints2(&objectPoints, &rotation, &translation, &camera, &distortion, imagePoints);
		printf("[%f, %f]", imagePoints->data.fl[0], imagePoints->data.fl[1]);
	
		// Check Conversation
		CvPoint point = Convert(&objectPoints, &camera, &rotation, &translation);


		// Show result on image
		cvNamedWindow("wnd");
		IplImage* img = cvLoadImage("C:\\Inco\\OpenCV2.1\\samples\\c\\left02.jpg");
		cvDrawCircle(img, cvPoint(imagePoints->data.fl[0], imagePoints->data.fl[1]), 5, CV_RGB(255,0,0));
		cvShowImage("wnd", img);
		cvWaitKey(0);
		cvReleaseImage(&img);


		//CvFileStorage* fs = cvOpenFileStorage( "temp.txt", 0, CV_STORAGE_WRITE );
		//cvWrite( fs, "camera_matrix", (void*)&camera );
		//cvWrite( fs, "objectPoints", (void*)&objectPoints );
		//cvWrite( fs, "rotation", (void*)&rotation );
		//cvWrite( fs, "translation", (void*)&translation );
		//cvWrite( fs, "distortion", (void*)&distortion );
		//cvWrite( fs, "imagePoints", (void*)&imagePoints );
		//cvReleaseFileStorage( &fs );

		cvReleaseMat(&imagePoints);
	}
};